#!/usr/bin/env python

import rospy
from gazebo_msgs.msg import ModelStates
import numpy as np

class ModelStateListener:
    def __init__(self):
        self.joint_positions = {}
        self.joint_velocities = {}
        rospy.Subscriber("/gazebo/model_states", ModelStates, self.callback)
        self.data=[0 for i in range(13)]

    def callback(self,data):
        # 打印每个模型的名字及其对应的位姿和速度
        for i in range(len(data.name)):
            model_name = data.name[i]
            if model_name=='aliengo_gazebo':
                position = data.pose[i].position
                orientation = data.pose[i].orientation
                linear_velocity = data.twist[i].linear
                angular_velocity = data.twist[i].angular
                # rospy.loginfo("  Position: x=%f, y=%f, z=%f", position.x, position.y, position.z)
                # rospy.loginfo("  Orientation: x=%f, y=%f, z=%f, w=%f", orientation.x, orientation.y, orientation.z, orientation.w)
                # rospy.loginfo("  Linear Velocity: x=%f, y=%f, z=%f", linear_velocity.x, linear_velocity.y, linear_velocity.z)
                # rospy.loginfo("  Angular Velocity: x=%f, y=%f, z=%f", angular_velocity.x, angular_velocity.y, angular_velocity.z)
                self.data=[position.x, position.y, position.z, orientation.x, orientation.y, orientation.z, orientation.w,
                         linear_velocity.x, linear_velocity.y, linear_velocity.z,  angular_velocity.x, angular_velocity.y, angular_velocity.z  ]

    def get_model_states(self):
        return self.data

if __name__ == "__main__":
    rospy.init_node('model_state_listener_node', anonymous=True)
    listener = ModelStateListener()
    
    rate = rospy.Rate(60)  # 10 Hz
    while not rospy.is_shutdown():
        listener.get_model_states()
        rate.sleep()
